#! /usr/bin/env python
'''
    这是解算全向轮姿态的一个工具类
    该类存在一个接口，即wheelParser()
    向该接口中传入速度与x轴的夹角以及速度大小还有角速度的大小，返回解析后的三个轮子的转速数组
    在解算的时候，根据动力学方程，需要小车中心到轮子中点的长度L，同时需要两个角度（从x轴正方向看，另外两个轮子的角度）
    theta1和theta2。其中这两个角度是固定的，不需要动态调整，但是那个距离要根据模型的不同而变化。
    这个L会写到配置文件中，之后可以从ros的参数服务器里面读取

    该函数采用的坐标系和 https://www.guyuehome.com/8378 中采用的坐标系相同，其中x轴正方向指向的是本小车的前方
'''
import rospy
import math
#自定义类
class TripleOmniWheelsParser():

 def __init__(self):
   #获取长度  TripleOmniLength: 0.2  定义pi值
   self.pi = 3.1415926535
   self.L = float(rospy.get_param("TripleOmniLength","0.2"))
   #设置两个固定角度大小
   self.theta1 = self.pi/3
   self.theta2 = self.pi/6
   #一些三角函数值是预先设定好的，为了节省时间就在这里直接计算
   self.cosTheta1 = math.cos(self.theta1)
   self.sinTheta1 = math.sin(self.theta1)
   self.cosTheta2 = math.cos(self.theta2)
   self.sinTheta2 = math.sin(self.theta2)

 def wheelParser(self,vel,angle,omega):
     #注意，这里vel是目标速度，angle是该速度和坐标系x轴的夹角（角度值，范围：0-360），omega是角速度
     #首先将角度转换成弧度
     rad = (angle/180)*self.pi
     #之后算出该速度在x,y方向上的分量
     vx = vel*math.cos(rad)
     vy = vel*math.sin(rad)
     #最后就可以计算三个电机的转速了
     va = vx+omega*self.L
     vb = (-1)*vx*self.cosTheta1-vy*self.sinTheta1+omega*self.L
     vc = (-1)*vx*self.sinTheta2+vy*self.cosTheta2+omega*self.L
     return [va,vb,vc,0]

 